
#include <Servo.h>
#include "InputFeedbackLoop.h"

const uint16_t bufSize = 25; //ALSO CHANGE InputFeedbackLoop BufferSize

// servos
Servo aileron; Servo elevator; Servo throttle; Servo rudder;
// servo output pin assignments
const int throttlePin = 10;
const int aileronPin = 11;
const int elevatorPin = 12;
const int rudderPin = 13;
const int accelPinX = 2;
const int accelPinY = 1;
const int accelPinZ = 0;
const int gyroPinX = 4;
const int gyroPinY = 5;
//const int compassPin = 8;

// sensor input buffers/averages
const float accelerometerInputSensativity = 2;
const float accelerometerAdjustmentSensativity = 6.0;
const float gyroInputSensativity = 5;
const float gyroAdjustmentSensativity = 3.0;
// integral sensitivity = 40 because 1 meter is 40 inches.
//const float integralSensitivity = .5;
// not currently being used.
const int maxCalibrationAdjust = 30;
const int inputSensativity = 2;

InputFeedbackLoop accelX = InputFeedbackLoop(accelPinX, accelerometerInputSensativity, accelerometerAdjustmentSensativity, maxCalibrationAdjust, inputSensativity);
InputFeedbackLoop accelY = InputFeedbackLoop(accelPinY, accelerometerInputSensativity, accelerometerAdjustmentSensativity, maxCalibrationAdjust, inputSensativity);
InputFeedbackLoop accelZ = InputFeedbackLoop(accelPinZ, accelerometerInputSensativity, accelerometerAdjustmentSensativity, maxCalibrationAdjust, inputSensativity);
InputFeedbackLoop gyroX = InputFeedbackLoop(gyroPinX, gyroInputSensativity, gyroAdjustmentSensativity, maxCalibrationAdjust, inputSensativity);
InputFeedbackLoop gyroY = InputFeedbackLoop(gyroPinY, gyroInputSensativity, gyroAdjustmentSensativity, maxCalibrationAdjust, inputSensativity);

// misc variables
int userInput = 0;
int count = 0;
boolean heliOn = true;
unsigned long lastAileron = millis();
unsigned long lastElevator = millis();

// output degrees
int inputThrottle = 0;
int inputAileron = 0;
int inputElevator = 0;
int inputRudder = 0;
int degreeThrottle = 0;
int degreeAileron = 90;
int degreeElevator = 90;
int degreeRudder = 90;

void calibrate() {
	
    accelX.reset();
    accelY.reset();
    accelZ.reset();
    gyroX.reset();
    gyroY.reset();
	
    accelX.Calibrate();
    accelY.Calibrate();
    accelZ.Calibrate();
    gyroX.Calibrate();
    gyroY.Calibrate();
	
}

void getUserInput() {
	// USER INPUT BLOCK
	userInput = Serial.read();
	if (userInput == 119 || userInput == 87) {
		inputThrottle += inputSensativity;
	} else if (userInput == 115 || userInput == 83) {
		inputThrottle -= inputSensativity;
	}// else {
    //inputThrottle = 0;
	//}
	
	if (userInput == 97 || userInput == 65) {
		inputRudder -= inputSensativity;
	} else if (userInput == 100 || userInput == 68) {
		inputRudder += inputSensativity;
	}// else {
	//  inputRudder = 0;
	//}
	
	if (userInput == 106 || userInput == 74) {
		inputAileron -= inputSensativity;
		lastAileron = millis();
	} else if (userInput == 108 || userInput == 76) {
		inputAileron += inputSensativity;
		lastAileron = millis();
	} else {
		if(millis() > lastAileron + 500){
			//inputAileron = 0;
		}
	}
	
	if (userInput == 105 || userInput == 73) {
		inputElevator -= inputSensativity;
		lastElevator = millis();
	} else if (userInput == 107 || userInput == 75) {
		inputElevator += inputSensativity;
		lastElevator = millis();
	} else {
		if(millis() > lastElevator + 500){
			//inputElevator = 0;
		}
	}
	
	if (userInput == 27 || userInput == 88 || userInput == 120) {
		heliOn = !heliOn;
		calibrate();
	}
	// END USER INPUT BLOCK
}

void getAverageSensorInputs() {
    accelX.ReadAndAverageOverBuffer();
    accelY.ReadAndAverageOverBuffer();
    accelZ.ReadAndAverageOverBuffer();
    gyroX.ReadAndAverageOverBuffer();
    gyroY.ReadAndAverageOverBuffer(); 
}

//long calculateOffset(InputFeedbackLoop feedback){
//	long returnDegreesOffset = (feedback.getFirstIntegrationSinceStart() * integralSensitivity);
//	if(returnDegreesOffset < -90){
//		returnDegreesOffset = -90;
//	}else if(returnDegreesOffset > 90){
//		returnDegreesOffset = 90;
//	}
//	return returnDegreesOffset;

//}

void calculateOutput() {
	degreeThrottle = inputThrottle;
	degreeAileron = (accelX.getDegrees() - (2 * gyroX.getFirstIntegrationSinceStart())); //currentChange will be considered the volocity since start.\u20ac
	degreeElevator = (accelY.getDegrees() + (2 * gyroY.getFirstIntegrationSinceStart())); //currentChange will be considered the volocity since start.
	//degreeAileron = (accelX.getDegrees() - (2 * (gyroX.getDegrees() - 90))); //currentChange will be considered the volocity since start.\u20ac
	//degreeElevator = (accelY.getDegrees() + (2 * (gyroY.getDegrees() - 90))); //currentChange will be considered the volocity since start.
	
        degreeRudder = inputRudder;
}

void setActuatorOutputs() {
	// OUTPUT BLOCK
	if (!heliOn){
                inputThrottle = 0;
                inputAileron = 0;
                inputElevator = 0;
                inputRudder = 90;
                
                degreeThrottle = 0;
                degreeAileron = 90;
                degreeElevator = 90;
                degreeRudder = 90;
	}

        throttle.write(inputThrottle);
	aileron.write(degreeAileron);
	elevator.write(degreeElevator);
	rudder.write(inputRudder);
	Servo::refresh();
	// END OUTPUT BLOCK
}
void debugOutputs(){
	Serial.print(" Throttle: ");
	Serial.print(inputThrottle); 
	Serial.print(" Aileron: ");
	Serial.print(degreeAileron); 
	Serial.print(" Elevator: ");
	Serial.print(degreeElevator); 
	Serial.print(" Rudder: ");
	Serial.print(inputRudder); 
}

void debugCode(InputFeedbackLoop &inputToDebug) {
	// DEBUG BLOCK
	//Serial.print(" Average: ");
	//Serial.print(inputToDebug.CurrentBufferAverage());  
	//Serial.print(" Calibration: ");
	//Serial.print(inputToDebug.Calibration());
	Serial.print(" LastInput: ");
	Serial.print(inputToDebug.LastInput());
	Serial.print(" First Degree: ");
	Serial.print(inputToDebug.getDegrees());
	Serial.print(" Second Degrees: ");
	Serial.print(inputToDebug.getFirstIntegrationSinceStart(), DEC);
	
	// END DEBUG BLOCK
}

void setup() {
	// attach digital outputs
	rudder.attach(rudderPin);
	elevator.attach(elevatorPin);
	aileron.attach(aileronPin);
	throttle.attach(throttlePin);
	setActuatorOutputs();
	// initialize buffers
	
	Serial.begin(115200);
	Serial.println("Ready");
	delay(1000); 
	
	calibrate();
}

void loop() {
	
	getUserInput();

	getAverageSensorInputs(); 

	calculateOutput();

	setActuatorOutputs();

	//debugOutputs();

	
	debugCode(accelX);
	//debugCode(gyroX);
	//debugCode(accelY);
	//debugCode(gyroY);
	Serial.println("");
	//delay(1);
	
	
}
